import serial

from dataframe import DataFrameIn, DataFrameOut, DF_HEAD

from pygame_scope import Scope

# ===== ===== ===== ===== =====
# 外观设置
# ===== ===== ===== ===== =====
# 初始化示波器（示波器也是程序窗口）
scope = Scope(sizex=800, sizey=600) # 窗口尺寸，单位像素

# 示波器基本设置（默认为 y-t 模式）
scope.axes.bgc = (0,0,0)    # 设置绘图区背景颜色（bg color）

yaxis = scope.axes.yaxis
yaxis.color = (255,255,0)       # 设置y轴的颜色
yaxis.font_color = (255,255,0)  # 设置y轴刻度字体的颜色
yaxis.font_size = 26            # 设置y轴刻度字体的大小
yaxis.lim_upper =  25            # 设置y轴数值上限(limit upper)
yaxis.lim_lower = -11            # 设置y轴数值下限
yaxis.tick_step = 7             # 每相邻两个刻度之间相差多少数值
yaxis.spline_thickness = 2      # 坐标轴线宽

scope.panel.min_value = -10       # 拖动条最小取值
scope.panel.max_value = 24       # 拖动条最大取值
scope.panel.value = 1          # 设置滑块的值
# val = scope.value             # 获取滑块的值

scope.axes.drawer.x_step = 5    # 设置x轴步长(x方向上，曲线上每两个采样点相距几个像素)


# x-y 模式设置（区别仅为 x-y 模式下会绘制 x轴）

# scope.axes.mode_xy = True                                     # step 1. 启用 x-y 模式
# cxy = scope.axes.create_curve([255,255,0], curve_type='x-y')  # step 2. 创建 x-y 曲线对象，注意curve_type参数
# cxy.append_sample((x, y))                                     # step 3. 向 x-y 曲线对象传入数值对
# cxy.n_points = 100        # 储存多少采样点
# cxy.connected = True      # 是否在将采样点用直线连起来
# cxy.draw_points = True    # 是否绘制采样点
# cxy.pt_radius = 2         # 采样点的半径
# cxy.thickness = 1         # 曲线线宽

xaxis = scope.axes.xaxis
xaxis.color = (255,255,0)       # 设置x轴的颜色
xaxis.font_color = (255,255,0)  # 设置x轴刻度字体的颜色
xaxis.lim_upper =  2            # 设置x轴数值上限(limit upper)
xaxis.lim_lower = -2            # 设置x轴数值下限
xaxis.tick_step = 1             # 每相邻两个刻度之间相差多少数值


# 程序中有一个默认的曲线对象，传给scope.append_sample(val)的值都通过这个对象绘制。
# 绘制两条以上的曲线，需通过c = scope.axes.create_curve(color)创建新的曲线对象，
# 然后调用c.append_sample(val)将数值绘制在新的曲线上。
scope.axes.default_curve.color = (0, 255, 255)  # 默认曲线的颜色


#暂不支持直接设置曲线线宽；字体大小可在对应类（YAxis、TrackBar）中修改
scope.apply()   # 使上面的设置生效


# ===== ===== ===== ===== =====
# Application Data
# ===== ===== ===== ===== =====
kp, ki, kd = 1, 1, 0
FRAME_SIZE_IN = 10


def i_sense(adc):
    volt = adc / 1024.0 * 5  # V
    i_il = volt / 22 * 14  # A
    return i_il


class App:
    ser = serial.Serial()
    ser.port = 'com10'
    ser.baudrate = 57600
    ser.timeout = 0.2
    
    c_target = scope.axes.create_curve([0, 225, 0])
    c_speed = scope.axes.create_curve([255, 0, 0])
    c_kp = scope.axes.create_curve([100]*3)
    c_ki = scope.axes.create_curve([175]*3)
    c_kd = scope.axes.create_curve([255]*3)
    c_p_item = scope.axes.create_curve([100, 0, 100])  # purpse
    c_i_item = scope.axes.create_curve([200, 200, 0])  # yellow
    c_sys_v = scope.axes.create_curve([0, 255, 0])
    c_ia = scope.axes.create_curve([249, 68, 23])  # orange
    c_ib = scope.axes.create_curve([250, 233, 22]) # yellow
    c_ic = scope.axes.create_curve([21, 251, 251]) # sky blue
    
    def __init__(self):
        self.ser.open()
        self.df_in = DataFrameIn(FRAME_SIZE_IN)
        
        self.speed_target = 0
    
    def __enter__(self):
        return self
    
    def __exit__(self, *arg, **kwarg):
        self.ser.close()
    
    def parse_draw(self):
        """ parse the dataframe and draw it"""
        _speed = self.df_in.bs_to_s16d1(0)
        _kp = self.df_in.bs_to_s16d1(2)
        _ki = self.df_in.bs_to_s16d1(4)
        _p_item = self.df_in.bs_to_s16d1(8)
        _i_item = self.df_in.bs_to_s16d1(10)
        _sys_v = self.df_in.bs_to_s16d1(14)
        _ia = self.df_in.bs_to_s16d1(16)
        _ib = self.df_in.bs_to_s16d1(18)
        _ic = self.df_in.bs_to_s16d1(20)
        
        print(_speed, _kp, _ki, _p_item, _i_item)
        print(_sys_v, _ia, _ib, _ic)
        self.c_target.append_sample(self.speed_target)
        self.c_speed.append_sample(_speed)
        self.c_kp.append_sample(_kp)
        # self.c_ki.append_sample(_ki)
        # self.c_p_item.append_sample(_p_item)
        # self.c_i_item.append_sample(_i_item)
        self.c_sys_v.append_sample(_sys_v)
        self.c_ia.append_sample(i_sense(_ia)) 
        self.c_ib.append_sample(i_sense(_ib))
        self.c_ic.append_sample(i_sense(_ic))
    
    def encode_send(self):
        df = DataFrameOut(8)
        df.s16d1_to_bs(0, self.speed_target)
        df.s16d1_to_bs(2, 1)
        df.s16d1_to_bs(4, 1)
        df.s16d1_to_bs(6, 1)
        
        self.ser.write(df.buf_to_send)
    
    def rotine(self):
        scope.show()  # display must be updated every loop
        
        # === read ===
        _bs = self.ser.read(1)
        
        print(_bs)
        if _bs == DF_HEAD:
            print('start')
            buf = self.ser.read(FRAME_SIZE_IN)
            if len(buf) == FRAME_SIZE_IN:
                self.df_in._buf = buf
                self.parse_draw()
            else:
                print('error')
                
        # === write ===
        if self.speed_target != scope.panel.value:
            self.speed_target = scope.panel.value
            print('set speed ', self.speed_target)
            self.encode_send()



class App2:
    ser = serial.Serial()
    ser.port = 'com11'
    ser.baudrate = 57600
    ser.timeout = 0.2
    
    c_spd_meas_drv = scope.axes.create_curve([0, 0, 255])
    c_spd_meas_drv.thickness = 3
    c_spd_meas_ctrl = scope.axes.create_curve([21, 251, 251]) # sky blue
    c_spd_meas_ctrl.thickness = c_spd_meas_drv.thickness -1
    c_spd_tar_drv = scope.axes.create_curve([255, 0, 0])
    c_spd_tar_ctrl = scope.axes.create_curve([255, 255, 0])  # yellow
    c_sys_v = scope.axes.create_curve([0, 255, 0])
    

    def __init__(self):
        self.ser.open()
        self.df_in = DataFrameIn(FRAME_SIZE_IN)
        
        self.speed_target = 0
    
    def __enter__(self):
        return self
    
    def __exit__(self, *arg, **kwarg):
        self.ser.close()
    
    def parse_draw(self):
        """ parse the dataframe and draw it"""
        _spd_meas_drv = self.df_in.bs_to_s16d1(0)
        _spd_tar_drv = self.df_in.bs_to_s16d1(2)
        _sys_v = self.df_in.bs_to_s16d1(4)
        _spd_meas_ctrl = self.df_in.bs_to_s16d1(6)
        _spd_tar_ctrl = self.df_in.bs_to_s16d1(8)
        
        print(_spd_meas_drv, _spd_tar_drv, _sys_v)
        print(_spd_meas_ctrl, _spd_tar_ctrl)
        self.c_spd_meas_drv.append_sample(_spd_meas_drv)
        self.c_spd_tar_drv.append_sample(_spd_tar_drv)
        self.c_sys_v.append_sample(_sys_v)
        self.c_spd_meas_ctrl.append_sample(_spd_meas_ctrl)
        self.c_spd_tar_ctrl.append_sample(_spd_tar_ctrl)
        
    
    def encode_send(self):
        df = DataFrameOut(2)
        df.s16d1_to_bs(0, self.speed_target)
        
        self.ser.write(df.buf_to_send)
    
    def rotine(self):
        scope.show()  # display must be updated every loop
        
        # === read ===
        _bs = self.ser.read(1)
        
        if _bs == DF_HEAD:
            print('start')
            buf = self.ser.read(FRAME_SIZE_IN)
            if len(buf) == FRAME_SIZE_IN:
                self.df_in._buf = buf
                self.parse_draw()
            else:
                print('error')
                
        # === write ===
        if self.speed_target != scope.panel.value:
            self.speed_target = scope.panel.value
            print('set speed ', self.speed_target)
            self.encode_send()
            


if __name__ == '__main__':
    # with serial.Serial('com6') as ser:
        # ser.baudrate = 57600
        
    with App2() as app:
        scope.mainloop(user_task=app.rotine)
    




